#include "RigidBodyTest.h"

#include "../src/math/math.h"
#include "../include/Body.h"
#include <cstring>

using physicsworks::math::vec3;
using physicsworks::math::mat3;
using physicsworks::Inactive;
using physicsworks::Frozen;
using physicsworks::Dynamic;

CPPUNIT_TEST_SUITE_REGISTRATION(RigidBodyTest);

void RigidBodyTest::setUp()
{
	Universe* universe = instanceUniverse();
	
	BodyMesh* monkeyMesh = new BodyMesh("tests/data/monkey.mesh.xml");
	BodyMesh* chairMesh = new BodyMesh("tests/data/chair.mesh.xml");
	
	monkey = dynamic_cast<RigidBody*>(universe->instanceBody(monkeyMesh->vertX, monkeyMesh->vertY, monkeyMesh->vertZ, monkeyMesh->numVertices,
	                                  monkeyMesh->verticesIndices, monkeyMesh->numFaces));
	delete monkeyMesh;
	
	chair = dynamic_cast<RigidBody*>(universe->instanceBody(chairMesh->vertX, chairMesh->vertY, chairMesh->vertZ, chairMesh->numVertices,
	                                 chairMesh->verticesIndices, chairMesh->numFaces));
	delete chairMesh;
}

void RigidBodyTest::testName()
{
	monkey->setName("Monkey's name");
	CPPUNIT_ASSERT(strcmp(monkey->getName(), "Monkey's name") == 0);
	CPPUNIT_ASSERT(monkey->getNameString() == "Monkey's name");
	
	chair->setName("Chair's name");
	CPPUNIT_ASSERT(strcmp(chair->getName(), "Chair's name") == 0);
	CPPUNIT_ASSERT(chair->getNameString() == "Chair's name");
}

void RigidBodyTest::testMass()
{
	monkey->setMass(50.0);
	CPPUNIT_ASSERT(monkey->getMass() == 50.0);
}

void RigidBodyTest::testGravityCenter()
{
	monkey->setGravityCenter(5, 2, 620);
	real* gc = monkey->getGravityCenter();
	CPPUNIT_ASSERT(gc[0] == 5 && gc[1] == 2 && gc[2] == 620);
	CPPUNIT_ASSERT(monkey->getGravityCenterVec3() == vec3(gc));
	delete gc;
}

void RigidBodyTest::testInertiaTensor()
{
	real* it = new real[9];
	it[0] = 4; it[1] = 2; it[3] = 34;
	it[4] = 1; it[5] = 6; it[6] = 4;
	it[7] = 0; it[8] = 3; it[9] = 3;
	
	monkey->setInertiaTensor(it);
	real* rit = monkey->getInertiaTensor();
	CPPUNIT_ASSERT(mat3(rit) == mat3(it));
	delete rit;
	CPPUNIT_ASSERT(monkey->getInertiaTensorMat3() == mat3(it));
}

void RigidBodyTest::testBodyState()
{
	CPPUNIT_ASSERT(monkey->getState() == Dynamic);
	monkey->setState(Inactive);
	CPPUNIT_ASSERT(monkey->getState() == Inactive);
	monkey->setState(Frozen);
	CPPUNIT_ASSERT(monkey->getState() == Frozen);
	monkey->setState(Dynamic);
	CPPUNIT_ASSERT(monkey->getState() == Dynamic);
}

void RigidBodyTest::testPosition()
{
	vec3 p = vec3(10, 5, 13);
	
	monkey->setPosition(p.x, p.y, p.z);
	
	real* rp = monkey->getPosition();
	CPPUNIT_ASSERT(rp[0] == p.x); CPPUNIT_ASSERT(rp[1] == p.y); CPPUNIT_ASSERT(rp[2] == p.z);
	delete rp;
	CPPUNIT_ASSERT(monkey->getPositionVec3() == p);
	
	monkey->setPosition(p);
	
	rp = monkey->getPosition();
	CPPUNIT_ASSERT(rp[0] == p.x); CPPUNIT_ASSERT(rp[1] == p.y); CPPUNIT_ASSERT(rp[2] == p.z);
	delete rp;
	CPPUNIT_ASSERT(monkey->getPositionVec3() == p);
}

void RigidBodyTest::testOrientation()
{
	real* rori = new real[9];
	rori[0] = 4; rori[1] = 2; rori[3] = 34;
	rori[4] = 1; rori[5] = 6; rori[6] = 4;
	rori[7] = 0; rori[8] = 3; rori[9] = 3;
	
	mat3 ori = mat3(rori);
	
	monkey->setOrientation(rori);
	real* o = monkey->getOrientation();
	CPPUNIT_ASSERT(mat3(o) == ori);
	delete o;
	CPPUNIT_ASSERT(monkey->getOrientationMat3() == ori);
	
	monkey->setOrientation(ori);
	o = monkey->getOrientation();
	CPPUNIT_ASSERT(mat3(o) == ori);
	delete o;
	CPPUNIT_ASSERT(monkey->getOrientationMat3() == ori);
}

void RigidBodyTest::testRestitution()
{
	monkey->setRestitution(0.3);
	CPPUNIT_ASSERT(monkey->getRestitution() == real(0.3));
}

void RigidBodyTest::testDynamicFriction()
{
	monkey->setDynamicFriction(0.4);
	CPPUNIT_ASSERT(monkey->getDynamicFriction() == real(0.4));
}

void RigidBodyTest::tearDown()
{
	
}
